﻿using UnityEngine;
using System;
using System.Collections;
using System.Collections.Generic;

class PriorityQueue<T>
{
    private IComparer<T> m_comparer;
    private T[] m_heap;

    private int m_count;
    public int Count {
        get
        {
            return m_count;
        }
        private set
        {
            m_count = value;
        }
    }

    public PriorityQueue() : this(null) { }
    public PriorityQueue(int capacity) : this(capacity, null) { }
    public PriorityQueue(IComparer<T> comparer) : this(16, comparer) { }

    public PriorityQueue(int capacity, IComparer<T> comparer)
    {
        m_comparer = (comparer == null) ? Comparer<T>.Default : comparer;
        m_heap = new T[capacity];
    }

    public void Push(T v)
    {
        if (m_count >= m_heap.Length)
        {
            Array.Resize(ref m_heap, m_count * 2);
        }
        m_heap[m_count] = v;
        SiftUp(m_count++);
    }

    public T Pop()
    {
        var v = Top();
        m_heap[0] = m_heap[--m_count];
        if (m_count > 0)
        {
            SiftDown(0);
        }
        return v;
    }

    public T Top()
    {
        if (m_count > 0)
        {
            return m_heap[0];
        }
        return default(T);
    }

    void SiftUp(int n)
    {
        var v = m_heap[n];
        for (int n2 = n / 2; n > 0 && m_comparer.Compare(v, m_heap[n2]) > 0; n = n2, n2 /= 2)
        {
            m_heap[n] = m_heap[n2];
        }
        m_heap[n] = v;
    }

    void SiftDown(int n)
    {
        var v = m_heap[n];
        for (var n2 = n * 2; n2 < m_count; n = n2, n2 *= 2)
        {
            if (n2 + 1 < m_count && m_comparer.Compare(m_heap[n2 + 1], m_heap[n2]) > 0)
            {
                n2++;
            }
            if (m_comparer.Compare(v, m_heap[n2]) >= 0)
            {
                break;
            }
            m_heap[n] = m_heap[n2];
        }
        m_heap[n] = v;
    }
}
